#pragma once
#ifndef AIRPLANE_HPP_
#define AIRPLANE_HPP_

#include <iostream>
#include <stdlib.h>
#include <Eigen/Geometry>
#include "Eigen/Dense"
#include <unordered_map>

#include <vector>

#include <algorithm>

struct Gimbal_Camera
{
    // //云台相机焦距
    // double f ;
    // //云台相机像素大小
    // double d ;
    double fx;
    double fy;
    double cx;
    double cy;
};

struct Rotation_Angles
{
    double roll;
    double pitch;
    double yaw;
};

struct Picture_Location {
    int u; // 像素位置 u
    int v; // 像素位置 v

    // 默认构造函数
    Picture_Location() 
        : u(0), v(0) // 初始化 u 和 v 为 0
    {}

    // 带参数的构造函数
    Picture_Location(int u_val, int v_val) 
        : u(u_val), v(v_val) // 初始化为指定值
    {}
};

struct TargetLostInfo
{
    TargetLostInfo()
    :TargetLostTimeMax(10000)
    {}

    // 上次捕捉到目标的时间
    uint64_t LastTimeFoundTarget;

    // // 目标丢失的时间长度
	// uint64_t TargetLostCount;

    // 该目标对应的最大时间间隔 ms, 超过该值则说明目标丢失
    uint64_t TargetLostTimeMax;

    // 对应id目标量测已丢失（计数超过最大值） 丢失：1 未丢失：0
    // 初始化为丢失状态
	bool TargetIsLost = true;
};

struct DDXTJC_Input
{
    uint64_t measure_time;
    Picture_Location picture_loc;
    Eigen::MatrixXd CoRotationMatrix;
    Eigen::Vector3d Camera_Location;
    Gimbal_Camera camera;
};

// 
struct Measurement_Pair {
    // 默认构造函数
    Measurement_Pair() 
        : Time_measure(0),// 初始化为 3x3 的零矩阵
         PictureLocation(0, 0),
         MeasurementMatrix(Eigen::MatrixXd::Zero(3, 3)),
         airplane_z(0)
    {}

    // 带维度的构造函数
    Measurement_Pair(int rows, int cols)
        : Time_measure(0),
         PictureLocation(0, 0),
         MeasurementMatrix(Eigen::MatrixXd::Zero(rows, cols)), // 初始化为指定尺寸的零矩阵
         airplane_z(0)
    {}

    // 获得当前观测量的时间 单位毫秒
    uint64_t Time_measure;
    // 当前观测所得的像素位置值
    Picture_Location PictureLocation;
    // 获得该观测时飞机所具有的观测矩阵
    Eigen::MatrixXd MeasurementMatrix;

    Eigen::Vector2d airplane_xy;
 
    double airplane_z;

    bool operator<(const Measurement_Pair &y) const {
        return Time_measure < y.Time_measure; // 设定按照时间由小到大排列
    }
};

// 该对象用于存储和计算飞机的测量结果及观测矩阵
class Airplane{
public:
    // 创建对象，需要
    // ②吊舱基座几何中心到无人机中心的平移向量tg
    // ④吊舱基座几何中心到无人机的旋转角度Rollg Pitchg Yawg，（通过外参标定获得），默认为0，生成相关旋转矩阵，默认为单位阵
    // ⑤云台相机焦距f和像素大小d
    // ⑥用于ukf的量测噪声方差矩阵R
	Airplane(Eigen::Vector3d t_g,
             Rotation_Angles Pod_Rotation_Angles,
             Gimbal_Camera Gimbal_camera_parameter,
             Eigen::MatrixXd Ri);

	~Airplane(){
	};

	// void Initialization(Eigen::Vector3d t_n, Eigen::Vector3d t_c, 
    //                     Rotation_Angles Gimbal_Rotation_Angles, Rotation_Angles Body_Rotation_Angles,
    //                     Picture_Location PictureLocation);

    // 是否接收过飞机的第一次数据
	bool Isinitalized(){ return is_initalized_;};//如果已初始化，则可执行各类Get方法

    // 若计算间隔内未能获得某个目标的数据，将对应目标丢失计数累加
    // 若累加计数超过最大值，则对应id目标量测已丢失，需清空对应目标所保存的所有非固定数据
    // 若长时间未能获得所有目标数据，则关闭初始化，清空所有非固定数据
    bool TargetIsLost(uint8_t TargetId);

    // 若所有飞机均完全丢失某个目标，则删除对应目标的丢失状态计数
    bool DeleteTarget(uint8_t TargetId);

    //// 由计算中心调用，以决定当前应执行S2/S3
    bool UpdateTargetLostInfo_Map_(uint8_t TargetId, uint64_t Time_measure);

    TargetLostInfo GetTargetLostInfo(uint8_t TargetId);

    void SetTargetLostInfoTargetLostTimeMax(uint8_t TargetId, uint64_t TargetLostTimeMax);

    // 接收观测数据，生成并保存相关数据结果，由后续Get函数获取结果
    // 
    // ①获得其云台俯仰、偏航欧拉角Pitchci Yawci，无人机滚转、俯仰、偏航欧拉角Rollbi Pitchbi Yawbi，生成相关旋转矩阵
    // ②从无人机重心到世界坐标系原点的平移向量tn，根据平移和旋转向量生成相关观测模型
    // 从相机光心到吊舱基座的几何中心的平移向量tc
    // ③当前观测量u, v，
    // ⑥获得观测量的时间
    // 目标id号
    // 更新观测矩阵
    // 如果没有发现目标，则更新目标丢失信息图
    // 初始化标志置true
    // 对应目标丢失计数重新置零
    // TargetIsAllLost = true时，执行S2
    bool UpdateAirplaneState(Eigen::Vector3d t_n, Eigen::Vector3d t_c,
                             Rotation_Angles Gimbal_Rotation_Angles, Rotation_Angles Body_Rotation_Angles,
                             uint8_t TargetId, bool TargetIsAllLost, 
                             Picture_Location PictureLocation, uint64_t Time_measure);

    // 获取某个目标的输出 返回有/无该目标相关数据， 同时清除当前飞机对该目标数据的存储
    // 首先判断是否初始化
    // 该方法调用时不允许添加新数据，由于每次处理均顺序执行update再运行S2或S3的计算，只要运行频率不过高，无需作保护
    // 用于S2 和 S3
    bool GetMeasurements_(std::vector<Measurement_Pair> &Measurements, uint8_t TargetId);

    //ukf量测噪声矩阵 用于 S3
    Eigen::MatrixX2d GetRi_();

    // 获取相机参数(用于S2)
    Gimbal_Camera GetGimbal_camera_parameter_();

    // 用于S2初始化，获取目标在最后一次量测时间下的，
    // 目标位置，旋转矩阵的连乘结果，相机光心相对世界坐标系原点的平移量，
    // 且该量测时间必须处于(当前时间-time_delta)之后。
    // 获取之后将清空该目标id所对应的当前量测集合
    // 首先判断该飞机是否初始化（获得过数据）
    // 该方法调用时不允许添加新数据，由于每次处理均顺序执行update再运行S2或S3的计算，只要运行频率不过高，无需作保护
    // time_delta可以为2倍算法运行间隔
    bool GetPicture_Location_(DDXTJC_Input &S2_Input,
                              uint8_t TargetId);

    //// 单独设置固定参数 
    //  ukf量测噪声矩阵，可实时修改，因为每次ukf迭代时只需取用该矩阵一次
    void SetRi_(Eigen::MatrixX2d Ri);
    void SetNoiseCov_u(float noiseu);
    void SetNoiseCov_v(float noisev);

    // 注意：修改以下固定参数时，飞机之前的所有量测将失效并被清除，不允许在测量计算的过程中对这类参数执行修改
    // 注意：设置角度的同时，需要同时设置旋转矩阵
    void SetPodTranslation_(Eigen::Vector3d t_g);
    void SetPodTranslation_x(float x);
    void SetPodTranslation_y(float y);
    void SetPodTranslation_z(float z);

    void SetPod_Rotation_Angles_(Rotation_Angles Body_Rotation_Angles);
    void SetPod_Rotation_Angles_roll(float roll);
    void SetPod_Rotation_Angles_pitch(float pitch);
    void SetPod_Rotation_Angles_yaw(float yaw);

    // 相机参数设置，可能在相机变倍时发生变化
    void SetGimbal_camera_parameter_(Gimbal_Camera Gimbal_camera_parameter);
    // void SetGimbal_camera_parameter_f(float f);
    // void SetGimbal_camera_parameter_d(float d);

    //// 设置实时输入参数
    // void SetCameraTranslation_(Eigen::Vector3d t_c);

    // void SetBodyTranslation_(Eigen::Vector3d t_n);

    // void SetGimbal_Rotation_Angles_(float pitch, float yaw);

    // void SetBody_Rotation_Angles_(float roll, float pitch, float yaw);

    //// 获取其他参数  首先判断是否初始化
    bool GetCamera_Translation_(Eigen::Vector3d &Camera_Translation);

    Eigen::Vector3d GetPod_Translation_();

    Rotation_Angles GetPod_Rotation_Angles_();

    // bool GetBody_Translation_(Eigen::Vector3d &Body_Translation);

    bool GetGimbal_Rotation_Angles_(Rotation_Angles &Gimbal_Rotation_Angles);

    //bool GetBody_Rotation_Angles_(Rotation_Angles &Body_Rotation_Angles);

	// 生成旋转矩阵
    Eigen::MatrixX3d GenerateRotationMatrix(Rotation_Angles angles);
    Eigen::MatrixX3d getRotationMatrixYaw(double yaw);
    Eigen::MatrixX3d getRotationMatrixPitch(double pitch);
    Eigen::MatrixX3d getRotationMatrixRoll(double roll);

    Eigen::MatrixXd computePseudoInverse(const Eigen::MatrixXd& matrix);

private:
    // 生成旋转矩阵的集合 用于S2
    Eigen::MatrixX3d GenerateCoRotationMatrix(Eigen::MatrixX3d Pod_RotationMatrix, Eigen::MatrixX3d Gimbal_RotationMatrix, Eigen::MatrixX3d Body_RotationMatrix);

    // 生成相机光心在世界坐标系下的位置 用于S2 
    Eigen::Vector3d GenerateCamera_Location_(Eigen::Vector3d t_g, Eigen::Vector3d t_c, Eigen::Vector3d t_n);

    // 生成观测矩阵模型H*z 3*4矩阵, 用于S3
    Eigen::MatrixXd GenerateMeasurementMatrix(Eigen::Vector3d t_g, Eigen::Vector3d t_c, Eigen::Vector3d t_n,
                                              Eigen::MatrixX3d Pod_RotationMatrix, Eigen::MatrixX3d Gimbal_RotationMatrix, Eigen::MatrixX3d Body_RotationMatrix,
                                              Gimbal_Camera Gimbal_camera_parameter);
    
    // 生成量测结果，用于S2 和 S3
    Measurement_Pair GenerateMeasurement(uint64_t Time_measure, Picture_Location PictureLocation, Eigen::MatrixXd MeasurementMatrix, Eigen::Vector2d airplane_xy, double z);

    // 保存目标在最后一次量测时间下的旋转矩阵的集合
    void SetCoRotationMatrix(Eigen::MatrixXd CoRotationMatrix, uint8_t id);

    // 保存相机光心在世界坐标系下的位置
    void SetCamera_Location_(Eigen::Vector3d Camera_Location,uint8_t id);

    // 保存量测结果
    // void SetMeasurements_(Measurement_Pair Measurement, uint8_t id);

    // void SetGimbal_RotationMatrix_(Rotation_Angles angles);

    // void SetBody_RotationMatrix_(Rotation_Angles angles);

    // 检测该模型是否初始化成功(拥有量测数据)
	bool is_initalized_ = false;

    //吊舱基座几何中心到无人机中心的平移向量tg
    Eigen::Vector3d Pod_Translation_;

    //吊舱基座几何中心到无人机的旋转角度Rollg Pitchg Yawg，
    Rotation_Angles Pod_Rotation_Angles_;

    //吊舱基座几何中心到无人机的旋转矩阵
    Eigen::MatrixX3d Pod_RotationMatrix_;

    //云台相机参数
    Gimbal_Camera Gimbal_camera_parameter_;

    //用于ukf的量测噪声方差矩阵Ri(每个飞机的矩阵分开)
    Eigen::MatrixX2d Ri_;

    // //// 实时输入参数 （作为中间量，不需要特别取用
    // // 对id目标的最后一次观测下，从相机光心到吊舱基座的几何中心的平移向量tc
    Eigen::Vector3d Camera_Translation_;

    // //从无人机重心到世界坐标系原点的平移向量tn
    // Eigen::Vector3d Body_Translation_;

     //云台俯仰，偏航欧拉角Rollc(永远为0，不可设置) Pitchci Yawci
     Rotation_Angles Gimbal_Rotation_Angles_;

    // //无人机旋转角度Rollb Pitchb Yawb，
    // Rotation_Angles Body_Rotation_Angles_;

    // ////实时计算参数 （作为中间量，不需要特别取用
    // //云台旋转矩阵
    // Eigen::MatrixX3d Gimbal_RotationMatrix_;

    // //无人机旋转矩阵
    // Eigen::MatrixX3d Body_RotationMatrix_;

    //// 实时变化并需要取用的参数
    //S2:
    //最后一次检测到目标时的相机光心在世界坐标系下的位置(飞机位置tn+吊舱偏移tc+光心偏移tg)
    std::unordered_map<uint8_t,Eigen::Vector3d> Camera_Location_;

    //最后一次检测到目标时的无人机旋转矩阵的连乘
    std::unordered_map<uint8_t,Eigen::MatrixXd> CoRotationMatrix_;
 
public: 
    //S2 S3:
    // map(目标id号，测量结果及其相关信息)
    // Computer取用相关数据后,不论用于S2还是S3，均需清除对应的所有测量数据（每次取用清除一次）
    std::unordered_map<uint8_t,std::vector<Measurement_Pair>> Measurements_;

    //对应id目标量测丢失信息（当前飞机下的量测情况,用于决定当前执行S2还是S3）
    std::unordered_map<uint8_t,TargetLostInfo> TargetLostInfo_Map_;
};
#endif



